relativeOccupationPercentile <- function(relativeOccupationMatrix,percentile,experimentDuration){
  percentileOccupation <- relativeOccupationMatrix[relativeOccupationMatrix["OccC1"]>=percentile &
                                                   relativeOccupationMatrix["OccC2"]>=percentile &
                                                   relativeOccupationMatrix["OccC3"]>=percentile &
                                                   relativeOccupationMatrix["OccC4"]>=percentile,1]
  
  # If a certain percentile is never reached, 
  # return the time corresponding to the simulation duration + 1
  # to signal infeasibily, without occuring in NA.
  if(length(percentileOccupation) == 0){
    return(10*as.numeric(experimentDuration)+1)
  }
  else{
    return(percentileOccupation)
  }
}

padVector <- function(vector,desiredLength){
  if(length(vector) < desiredLength){
    paddingLength = desiredLength - length(vector)
    return(append(vector,rep(vector[length(vector)],paddingLength)))
  }
  return(vector)
}

addErrorBars <- function(x,yplus,yminus,intersamples,color){
  samples <- seq(from=0,to=length(x),by=intersamples)
  arrows(x[samples],yplus[samples], x[samples], yminus[samples],code=3,angle=90,length="0.1",col=color)
}


computeMaxError <- function(inputFile,experimentDuration){
  #Read data and compute errors for each cluster
  inputData <- read.table(inputFile, header = TRUE)
  
  #Compute max error in each cluster -> max_i(e_i(t))
  errorMatrix <- cbind(inputData[2]-inputData[3],inputData[4]-inputData[5],inputData[6]-inputData[7],inputData[8]-inputData[9])
  maxErrorVector <- apply(errorMatrix,1,max)
  print(errorMatrix[dim(errorMatrix)[1],])
  
  #Integrate error by:
  #1. Computing (f(i+1)-f(i))/2
  #maxErrorShifted <- cbind(as.ts(maxErrorVector),lag(maxErrorVector,-1))
  #maxErrorIntegrated <- (maxErrorShifted[,1] + maxErrorShifted[,2])*0.5
  
  #2. 
  #a. e_i(0) is given
  #maxErrorIntegrated[1] <- maxErrorShifted[1,1]
  #b. The following increments are computed by means of cumulative sums
  #maxErrorIntegrated <- cumsum(maxErrorIntegrated[1:(length(maxErrorIntegrated)-1)]) / inputData[1]
  #colnames(maxErrorIntegrated)[1] <- expression(max_i(e_i(t)))
  
  #return(maxErrorIntegrated)
  returnVector <- padVector(maxErrorVector,10*as.numeric(experimentDuration))
  return(returnVector)
}

computeOccupationTimes <- function(inputFile,experimentDuration){
  #Read data and compute errors for each cluster
  inputData <- read.table(inputFile, header = TRUE)
  
  #Compute relative occupation of the clusters: \frac{o_i}{r_i}
  relativeOccupationMatrix <-cbind(inputData[1],inputData[3]/inputData[2],inputData[5]/inputData[4],inputData[7]/inputData[6],inputData[9]/inputData[8])
 
  
  firstQuartileOccupation <- relativeOccupationPercentile(relativeOccupationMatrix,0.25,experimentDuration)
  secondQuartileOccupation <- relativeOccupationPercentile(relativeOccupationMatrix,0.50,experimentDuration)
    
  returnValues <- list(firstQuartileOccupation[1],secondQuartileOccupation[1])
  return(returnValues)
}

# processWalkingRobots <- function(inputFile){
#   #Read data and compute errors for each cluster
#   inputData <- read.table(inputFile, header = TRUE)
#   
#   return(inputData$Walking)
# }
# 
# processDirectingRobots <- function(inputFile){
#   #Read data and compute errors for each cluster
#   inputData <- read.table(inputFile, header = TRUE)
#   
#   return(inputData$Directing)
# }

processWorkingRobots <- function(inputFile,experimentDuration){
  #Read data and compute errors for each cluster
  inputData <- read.table(inputFile, header = TRUE)
  print(inputData[dim(inputData)[1],c(10,11,12,13)])
  if(dim(inputData)[1] == 10*as.numeric(experimentDuration)){
    write(paste("Check: ",inputFile," -> Wa:",inputData[dim(inputData)[1],10]," As: ",inputData[dim(inputData)[1],11]," Di: ",inputData[dim(inputData)[1],12]," Wo: ",inputData[dim(inputData)[1],13]),stderr())
  }
  return(padVector(inputData$Working,10*as.numeric(experimentDuration)))
}

extractCompletionTime <- function(inputFile){
  #Read data and compute errors for each cluster
  inputData <- read.table(inputFile, header = TRUE)
  return(inputData[dim(inputData)[1],1])
}

plotMaxError <- function(integratedErrorsPlot,experimentDuration){
  #Plot max error 
  pdf("MaxError.pdf",family="Droid Sans")
  xRange <- range(0,dim(integratedErrorsPlot)[1])
  yRange <- range(0,8)
  nPlots <- dim(integratedErrorsPlot)[2]
  plotColors <- c("black","white","grey25","white","black","gray50","black") 
  lineType <- c(2,0,5,0,2,4,1) 
  plotChar <- seq(18,18+nPlots,1)
  
  plot(xRange,yRange, type="n", bty="l" , xlab="Time steps", ylab="Robots",main=paste("Maximum error across clusters on", length(resultsFiles),"trials"))   
  
  #Add lines
  polygon(c(seq(0,9999),seq(9999,0)),c(integratedErrorsPlot[,2],rev(integratedErrorsPlot[,4])),col="grey75",border=NA)
  matlines(integratedErrorsPlot, type="l" , lwd=1.5, lty=lineType, col=plotColors, pch=plotChar)
  #lines(integratedErrorsPlot[3],lty=lineType, col=plotColors, pch=plotChar)
  #addErrorBars(x=seq(from=0,to=dim(integratedErrorsPlot)[1]),yplus=integratedErrorsPlot[5],yminus=integratedErrorsPlot[1],intersample=200,color=plotColors)
  #Add a title
  #title(graphTitle)
  
  #Add a legend 
  legend(x="bottomright", c("Min-Max","Median","Mean","Optimal"), horiz=TRUE, cex=0.69, col=c("black","grey25","gray50","black"), lty=c(2,5,4,1), inset=c(0,.01))
  dev.off()
  
  write.table(integratedErrorsPlot,"MaxErrorPlots.cmp",sep="\t",row.names=FALSE,col.names=TRUE)
  #plot.ts(meanIntegratedMaxError,ylab=expression(max(e[i](t),i)))
  #title(str(graphTitle))
  #dev.off()
}

plotRobotStates <- function(robotStates){
  #Plot max error 
  pdf("RobotStates.pdf",family="Droid Sans")
  graphTitle <- paste("Number of allocated robots across",length(resultsFiles),"trials")
  xRange <- range(0,dim(robotStates)[1])
  yRange <- range(0,20)
  nPlots <- 3
  #plotColors <- rainbow(nPlots) 
  #lineType <- c(1,2,4,2,1) 
  plotColors <- c("black","white","grey25","white","black","gray50") 
  lineType <- c(2,0,5,0,2,4)
  plotChar <- seq(18,18+nPlots,1)
  
  plot(xRange,yRange, type="n", bty="l" , xlab="Time steps", ylab="Robots")   
   
  #Add lines
  polygon(c(seq(0,9999),seq(9999,0)),c(robotStates[,2],rev(robotStates[,4])),col="grey75",border=NA)
  matlines(robotStates, type="l" , lwd=1.5, lty=lineType, col=plotColors, pch=plotChar)
  
  #Add a title
  title(graphTitle)
  
  #Add a legend 
  #legend(x="bottomright", colnames(workingPlot), cex=0.8, col=plotColors, lty=lineType, title="Percentiles")
  legend(x="bottomright", c("Min-Max","Median","Mean"), horiz=TRUE, cex=0.69, col=c("black","grey25","gray50"), lty=c(2,5,4), inset=c(0,.01))
  dev.off()
  
}

plotDistribution <- function(occupationTimes){
  plot(occupationTimes)
  pdf("Distribution25.pdf",family="Droid Sans")
  #Plot empirical CDF of occupation times - 25% 
  plot(ecdf(occupationTimes[,1]),col="orange",bty="l",xlab="Time steps",main=paste("Time needed to reach 25% occupation across",length(resultsFiles),"trials"))
  dev.off()
  
  pdf("Distribution50.pdf",family="Droid Sans")
  #Plot empirical CDF of occupation times - 50% 
  plot(ecdf(occupationTimes[,2]),col="blue",bty="l",xlab="Time steps",main=paste("Time needed to reach 50% occupation across",length(resultsFiles),"trials"))
  dev.off()
  
  #pdf("Distribution75.pdf")
  #Plot empirical CDF of occupation times - 75%  
  #plot(ecdf(occupationTimes[,3]),col="green",xlab="Time steps",main=paste("Time needed to reach 75% occupation across",length(resultsFiles),"trials"))
  #dev.off()
  
  write.table(occupationTimes,"CDFDistributions.cmp",sep="\t",row.names=FALSE,col.names=TRUE)
}

createTexGraph <- function(xlabel,ylabel,height,width,xdataset,ydataset){
  #Output values to console -> Redirect to file to obtain a valid tex file
  sink("./results/Occupation.tex",append=TRUE)
  sprintf("\\begin{tikzpicture}\n")
  sprintf("  \\begin{axis}[\n")
  sprintf("	xlabel=%s,\n",xlabel)
  sprintf("	ylabel=%s$,\n",ylabel)
  sprintf("	height=%fcm,\n",height)
  sprintf("	width=%fcm,\n",width)
  sprintf("	grid=major,\n")
  sprintf("	legend style={at={(0,0)},\n")
  sprintf("	anchor=north,legend columns=-1},")
  sprintf("	]\n") 
  sprintf("\\addplot [color=green,mark=none,very thick] coordinates {\n")
  sprintf("(%d,%f)\n",xdataset,ydataset)
  sprintf("    };\n")
  #sprintf("\\addlegendentry{"+str(legend[index])+"}\n")
  #sprintf("\\legend{")
  #for element in legend:
  #  sprintf(str(element)+",")
  #sprintf("}\n") 
  sprintf("	\\end{axis}\n")
  sprintf("\\end{tikzpicture}\n")
}

#computeQuantileByRow <- function(matrix){
#  return t(apply(matrix,1,quantile))
#}
#############################################################################
library(extrafont)
library(Hmisc)

# $1 - Directory where the .occ files corresponding to the different experiments are stored
# $2 - Experiment duration (seconds)
args <- commandArgs(trailingOnly = TRUE)
workingDirectory <- args[1]
experimentDuration <- args[2]
rm(args)

#Set the directory containing the experiment results as current working directory
setwd(paste("/home/deste/argos2/user/jdestefani/results/",workingDirectory,"/",sep=""))
#List all the files containing information about occupation
resultsFiles <- list.files(pattern = "\\.occ$")

print("[STATUS] - Processing results file")
#Process every single file separately
maxErrors <- lapply(resultsFiles,computeMaxError,experimentDuration=experimentDuration)
occupationTimes <- lapply(resultsFiles,computeOccupationTimes,experimentDuration=experimentDuration)
#walkingPlot <- lapply(resultsFiles,processWalkingRobots)
#directingPlot <- lapply(resultsFiles,processDirectingRobots)
workingPlot <- sapply(resultsFiles,processWorkingRobots,experimentDuration=experimentDuration)
#Transform processing results into dataframe
maxErrors <- data.frame(matrix(unlist(maxErrors), ncol=length(maxErrors)))
occupationTimes <- data.frame(matrix(unlist(occupationTimes), nrow=length(occupationTimes), byrow = T))
# Extract only the times that are smaller than 10000
occupation25 <- sort(occupationTimes[occupationTimes[,1]<=10000,1])
occupation50 <- sort(occupationTimes[occupationTimes[,2]<=10000,2])

# Compute the corresponding values to draw the step functions
if(length(occupation25)>0){
  step25 <- seq(1:length(occupation25))/length(occupationTimes[,1])
}else{
  occupation25 <- c(0)
  step25 <- c(0)
}


if(length(occupation50)>0){
  step50 <- seq(1:length(occupation50))/length(occupationTimes[,2])
}else{
  occupation50 <- c(0)
  step50 <- c(0)
}

output25 <- data.frame(occupation25,step25)
output50 <- data.frame(occupation50,step50)

#walkingPlot <- data.frame(matrix(unlist(walkingPlot), ncol=length(walkingPlot)))
#directingPlot <- data.frame(matrix(unlist(directingPlot), ncol=length(directingPlot)))
#workingPlot <- data.frame(matrix(unlist(workingPlot), ncol=length(workingPlot)))
completionTimeDistribution <- sapply(resultsFiles,extractCompletionTime)

print("[STATUS] - Computing quantiles & mean")
#Given the matrix containing the integrated errors for each trial as column, compute the quantiles per row.
#That corresponds to obtain the quantiles at each time step
maxErrorsPlot <- t(apply(maxErrors,1,quantile))
workingPlot <- t(apply(workingPlot,1,quantile))
#Compute row by row mean of the integrated error of every single trial
meanMaxError <- apply(maxErrors,1,mean)
meanWorkingPlot <-apply(workingPlot,1,mean)

# Sample values at regular intervals for statistical testing
sampleIndex <- seq(from=1,to=10*as.numeric(experimentDuration),by=50)
samples <- data.frame(maxErrorsPlot[sampleIndex,3],meanMaxError[sampleIndex],workingPlot[sampleIndex,3],meanWorkingPlot[sampleIndex])

print("[STATUS] - Determining optimal allocation value")
#Compute optimal allocation value and plot
robotNumber <- 20
requests <- 25
clusters <- 4

optimalAllocation <- (requests-robotNumber)%%clusters + 1
optimalAllocation <- rep(optimalAllocation,dim(maxErrorsPlot)[1])
maxErrorsPlot <- cbind(maxErrorsPlot,meanMaxError,optimalAllocation)
colnames(maxErrorsPlot) <- c("0%","25%","50%","75%","100%","Mean","Optimal")
workingPlot <- cbind(workingPlot,meanWorkingPlot)
colnames(workingPlot) <- c("0%","25%","50%","75%","100%","Mean")

print("[STATUS] - Producing plots")
#plotMaxError(integratedErrors)
plotMaxError(maxErrorsPlot)
print(occupationTimes)
plotRobotStates(workingPlot)
plotDistribution(occupationTimes)

print("[STATUS] - Producing exchange files")

write.table(output25,paste("1",workingDirectory,"o25",sep="."),sep="\t",row.names=FALSE,col.names=TRUE)
write.table(output50,paste("1",workingDirectory,"o50",sep="."),sep="\t",row.names=FALSE,col.names=TRUE)
write.table(completionTimeDistribution,paste(workingDirectory,"bp",sep="."),sep="\t",row.names=FALSE,col.names=TRUE)
write.table(samples,paste(workingDirectory,"samples",sep="."),sep="\t",row.names=FALSE,col.names=TRUE)
#Plot integrated max error 
#pdf("IntegratedMaxError.pdf")
#graphTitle <- paste("Integrated Max Error averaged across",length(resultsFiles),"trials")
# xRange <- range(timeScale)
# yRange <- range(0,1)
# nPlots <- dim(data)[2]
# plotColors <- rainbow(nPlots) 
# lineType <- c(1:nPlots) 
# plotChar <- seq(18,18+nPlots,1)
# 
# plot(xRange,yRange, type="n", xlab="Time (s)", ylab=expression(p) , log = 'x' )   
# 
# # add lines
# #matlines(timeScale, data, type="l" , lwd=1.5, lty=lineType, col=plotColors, pch=plotChar)
# 
# #Add a title
# #title(stringTitle)
# 
# # add a legend 
# #legend(xRange[1], yRange[2], legend, cex=0.8, col=plotColors, pch=plotChar, lty=lineType, title="QRTD")
# #dev.off()
#plot.ts(meanIntegratedMaxError,ylab=expression(max(e[i](t),i)))
#title(str(graphTitle))
#dev.off()

#boxplot(meanIntegratedMaxError)
#title(main="Integrated Max Error Distribution?")
# Create a title with a red, bold/italic font

#Output analysis result
#sprintf("Time required to get 25%% coverage: %.0f",firstQuartileOccupation[1])
#sprintf("Time required to get 50%% coverage: %.0f",secondQuartileOccupation[1])
#sprintf("Time required to get 75%% coverage: %.0f",thirdQuartileOccupation[1])

rm(maxErrors)
rm(occupationTimes)
rm(meanMaxError)
rm(resultsFiles)

#createTexGraph("t","$\\max_i(e_i(t))$",5,5,inputData[,1],maxErrorIntegrated[,1])
